/**
 * @file thread8.c
 * @brief TU-BD100组合导航传感器数据接收线程实现
 * 
 * 该线程负责接收TU-BD100组合导航传感器的INSPVAXA数据。主要功能包括：
 * 1. 初始化RS232串口接口（/dev/ttyS8，对应RS232-1，460800 8-N-1）
 * 2. 发送配置命令：log com1 inspvaxa ontime 2（每2秒输出一条INSPVAXA数据）
 * 3. 接收并解析INSPVAXA格式的导航报文
 * 4. 详细解析并打印所有INSPVAXA字段信息
 * 5. 将解析后的数据存储在全局结构体中
 * 6. 提供接口供其他线程获取导航数据
 * 
 * @author 伞控无人机项目组
 * @date 2023-2024
 */

/**
 * @brief TU-BD100组合导航设备通信协议说明
 * 
 * =========================================================
 * 设备配置
 * =========================================================
 * 波特率：460800 8-N-1
 * 协议：仅支持INSPVAXA
 * 配置命令：log com1 inspvaxa ontime 2  // 每2秒输出一条INSPVAXA数据
 * 
 * =========================================================
 * INSPVAXA报文格式
 * =========================================================
 * 报文示例：
 * #INSPVAXA,COM1,0,73.5,FINESTEERING,1695,309428.000,00000040,4e77,43562;INS_SOLUTION_GOOD,INS_PSRSP,51.11637873403,-114.03825114994,1063.6093,-16.9000,-0.0845,-0.0464,-0.0127,0.138023492,0.069459386,90.000923268,0.9428,0.6688,1.4746,0.0430,0.0518,0.0521,0.944295466,0.944567084,1.000131845,3,0*e877c178
 * 
 * 报文字段说明：
 * 1. 语句头：以#INSPVAXA开始，以分号(;)结束
 * 2. INS状态字：组合导航工作状态字
 *    - INS_INACTIVE：IMU语句在输出，但是对准工作没开始，INS工作状态不可用
 *    - INS_ALIGNING：INS设备还在对准过程中
 *    - INS_HIGH_VARIANCE：对准完成，组合导航航向还未收敛完成
 *    - INS_SOLUTION_GOOD：组合导航收敛完成，工作状态良好
 *    - INS_SOLUTION_FREE：卫星导航定位误差过大，INS拒绝卫导参与解算
 *    - INS_ALIGNMENT_COMPLETE：INS设备对准完成，但是精度不够，仍需动态收敛
 *    - DETERMINING_ORIENTATION：INS正在用重力进行对准收敛
 *    - WAITING_INITIALPOS：INS等待获取初始位置，获取后才能开始对准
 * 3. POS状态字：当前定位工作状态字
 *    - NONE：无状态，不可用
 *    - Single：单点定位状态，无差分信号
 *    - INS_PSRDIFF：处于伪距差分状态，精度在0.3m左右
 *    - INS_RTKFLOAT：处于浮点解状态，精度在分米级别高于伪距
 *    - INS_RTKFIXED：处于固定解状态，精度在0.02m左右
 * 4. 纬度：WGS84坐标系下的纬度
 * 5. 经度：WGS84坐标系下的经度
 * 6. 高程：WGS84坐标系下的海拔高
 * 7. 起伏（椭球分离值）：海拔高与大地高的差值
 * 8. 北向速度：北方向的速度m/s
 * 9. 东向速度：东方向的速度m/s
 * 10. 天向速度：朝天方向的速度m/s
 * 11. 横滚：横滚角，单位：度
 * 12. 俯仰：俯仰角，单位：度
 * 13. 航向：航向角，单位：度
 * 14. 纬度标差：纬度值标准差，纬度误差值
 * 15. 经度标差：经度值标准差，经度误差值
 * 16. 高程标差：高程值标准差，高程误差值
 * 17. 北速标差：北向速度标准差
 * 18. 东速标差：东向速度标准差
 * 19. 天速标差：天向速度标准差
 * 20. 横滚标差：横滚角度标准差
 * 21. 俯仰标差：俯仰角度标准差
 * 22. 航向标差：航向角度标准差
 * 23. 扩展态：扩展状态字
 * 24. 更新时间：设备静止的时间长
 * 25. CRC校验和：以*开头的校验和，使用CRC32算法
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/types.h>
#include <pthread.h>
#include <stdint.h>
#include <ctype.h>
#include <math.h>
#include <sys/ioctl.h>  // 添加ioctl头文件，支持串口控制
#include <linux/serial.h> // 添加串口结构体头文件
#include "thread8.h"

// 全局变量定义
static int g_thread_running = 1;     // 线程运行标志
static int g_nav_rs232_fd = -1;      // 导航串口文件描述符
static int g_last_frame_count = 0;   // 上一个IMU帧计数
static int g_imu_lost_frames = 0;    // IMU丢失帧数统计
static int g_total_imu_frames = 0;   // IMU总帧数统计
static time_t g_imu_stat_time = 0;   // IMU统计开始时间
static time_t last_data_time = 0;    // 记录上次成功接收数据的时间

// 全局导航数据
nav_rmc_data_t g_nav_rmc_data;
nav_gga_data_t g_nav_gga_data;
nav_vtg_data_t g_nav_vtg_data;
nav_inspvaxa_data_t g_nav_inspvaxa_data;
nav_gpfpd_data_t g_nav_gpfpd_data;
nav_imu_data_t g_nav_imu_data;
pthread_mutex_t g_nav_data_mutex = PTHREAD_MUTEX_INITIALIZER;

// 内部函数声明
static int nav_read_serial(int fd, uint8_t *buffer, int buffer_size);
static int nav_parse_frame(uint8_t *buffer, int length);
static int nav_parse_rmc(char *frame);
static int nav_parse_gga(char *frame);
static int nav_parse_vtg(char *frame);
static int nav_parse_inspvaxa(char *frame);
static int nav_parse_gpfpd(char *frame);
static int nav_parse_imu_data(char *frame);
static double parse_coordinate(const char *str);
static int check_checksum(const char *frame);
static int is_frame_complete(const char *frame, int length);
static int wait_for_frame_complete(int fd, uint8_t *buffer, int buffer_size, int *received_length);
static int extract_inspvaxa_frame(const uint8_t *buffer, int length, char *frame_buffer, int frame_buffer_size);

/**
 * @brief 设置导航报文输出频率
 * 
 * @param fd 串口文件描述符
 * @param msg_type 报文类型
 * @param rate 输出频率(Hz)
 * @return 成功返回0，失败返回-1
 */
int set_nav_output_rate(int fd, const char *msg_type, float rate) {
    char cmd[100];
    int ret;
    
    // 检查参数
    if (fd < 0 || msg_type == NULL || rate <= 0) {
        return -1;
    }
    
    // 构建命令: log com1 [msg_type] ontime [1/rate]
    // 例如: log com1 inspvaxa ontime 0.05  (20Hz)
    float period = 1.0f / rate;
    snprintf(cmd, sizeof(cmd), "log com1 %s ontime %.3f\r\n", msg_type, period);
    
    // 发送命令
    ret = write(fd, cmd, strlen(cmd));
    if ((size_t)ret != strlen(cmd)) {
        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "发送导航报文配置命令失败: %s", strerror(errno));
        return -1;
    }
    
    LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "配置导航报文 %s 输出频率为 %.1f Hz", msg_type, rate);
    
    // 等待一小段时间，确保命令被处理
    usleep(100000);  // 100ms
    
    return 0;
}

/**
 * @brief 初始化导航串口
 * 
 * @param device 串口设备名
 * @param baudrate 波特率
 * @return 成功返回文件描述符，失败返回-1
 */
int nav_rs232_init(const char *device, int baudrate) {
    int fd;
    struct termios options;
    
    // 打开串口设备
    fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd < 0) {
        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "打开导航串口设备失败: %s", strerror(errno));
        return -1;
    }
    
    // 设置为阻塞模式
    fcntl(fd, F_SETFL, 0);
    
    // 获取当前串口参数
    if (tcgetattr(fd, &options) != 0) {
        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "获取导航串口参数失败: %s", strerror(errno));
        close(fd);
        return -1;
    }
    
    // 设置波特率
    cfsetispeed(&options, baudrate);
    cfsetospeed(&options, baudrate);
    
    // 设置控制模式
    options.c_cflag |= (CLOCAL | CREAD);  // 忽略调制解调器控制线，启用接收器
    options.c_cflag &= ~PARENB;           // 无奇偶校验
    options.c_cflag &= ~CSTOPB;           // 1个停止位
    options.c_cflag &= ~CSIZE;            // 清除数据位掩码
    options.c_cflag |= CS8;               // 8个数据位
    
    // 设置本地模式
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);  // 原始模式
    
    // 设置输入模式
    options.c_iflag &= ~(IXON | IXOFF | IXANY);  // 禁用软件流控制
    options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);  // 禁用特殊字符处理
    
    // 设置输出模式
    options.c_oflag &= ~OPOST;  // 禁用输出处理
    
    // 设置超时参数
    options.c_cc[VMIN] = 0;    // 不等待字符
    options.c_cc[VTIME] = 1;   // 超时0.1秒
    
    // 应用设置
    if (tcsetattr(fd, TCSANOW, &options) != 0) {
        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "设置导航串口参数失败: %s", strerror(errno));
        close(fd);
        return -1;
    }
    
    // 检查和配置串口缓冲区大小
    struct serial_struct ser_info;
    const int target_size = 65536; // 64K字节
    
    // ===== 配置前：获取当前缓冲区大小 =====
    if (ioctl(fd, TIOCGSERIAL, &ser_info) == 0) {
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "===== 配置前缓冲区大小 =====");
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "%s 当前发送缓冲区: %d 字节", device, ser_info.xmit_fifo_size);
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "%s 当前接收缓冲区: %d 字节", device, ser_info.xmit_fifo_size);
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "串口类型: %d, 端口: 0x%x, IRQ: %d", 
                 ser_info.type, ser_info.port, ser_info.irq);
        
        // 某些特殊系统可能支持单独设置接收缓冲区
        #ifdef CONFIG_SERIAL_SEPARATE_RECV_BUFFER
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "接收缓冲区(独立): %d 字节", ser_info.recv_fifo_size);
        #endif
    } else {
        LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "无法获取串口信息: %s", strerror(errno));
        goto skip_buffer_config;
    }
    
    // ===== 正在配置为64K缓冲区 =====
    LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "===== 正在配置为64K缓冲区 =====");
    
    // 先获取当前配置
    if (ioctl(fd, TIOCGSERIAL, &ser_info) == 0) {
        // 保存原始缓冲区大小
        int original_size = ser_info.xmit_fifo_size;
        
        // 设置新的缓冲区大小为64K
        ser_info.xmit_fifo_size = target_size;
        
        // 某些系统支持单独设置接收缓冲区
        #ifdef CONFIG_SERIAL_SEPARATE_RECV_BUFFER
        ser_info.recv_fifo_size = target_size;
        #endif
        
        // 应用新的串口配置
        if (ioctl(fd, TIOCSSERIAL, &ser_info) == 0) {
            LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "成功设置串口缓冲区大小为: %d 字节", target_size);
        } else {
            LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "设置64K缓冲区失败: %s", strerror(errno));
            LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "可能系统不支持这么大的缓冲区，保持原始大小: %d 字节", original_size);
        }
    } else {
        LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "获取当前配置失败: %s", strerror(errno));
    }
    
    // ===== 配置后：验证设置结果 =====
    if (ioctl(fd, TIOCGSERIAL, &ser_info) == 0) {
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "===== 配置后缓冲区大小 =====");
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "%s 验证后发送缓冲区: %d 字节", device, ser_info.xmit_fifo_size);
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "%s 验证后接收缓冲区: %d 字节", device, ser_info.xmit_fifo_size);
        
        // 某些特殊系统可能支持单独设置接收缓冲区
        #ifdef CONFIG_SERIAL_SEPARATE_RECV_BUFFER
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "接收缓冲区(独立): %d 字节", ser_info.recv_fifo_size);
        #endif
        
        // 检查是否成功设置为64K
        if (ser_info.xmit_fifo_size == target_size) {
            LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "成功: 缓冲区已设置为64K字节");
        } else {
            LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "注意: 缓冲区实际大小为 %d 字节，未达到64K", ser_info.xmit_fifo_size);
        }
    } else {
        LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "无法验证串口缓冲区大小: %s", strerror(errno));
    }
    
skip_buffer_config:
    
    LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "TU-BD100串口 %s 初始化成功，波特率: 460800 8-N-1", device);
    return fd;
}

/**
 * @brief 从串口读取数据
 * 
 * @param fd 串口文件描述符
 * @param buffer 接收缓冲区
 * @param buffer_size 缓冲区大小
 * @return 实际接收的数据长度，错误返回-1
 */
__attribute__((unused))
static int nav_read_serial(int fd, uint8_t *buffer, int buffer_size) {
    int ret;
    
    // 检查参数
    if (fd < 0 || buffer == NULL || buffer_size <= 0) {
        return -1;
    }
    
    // 读取数据
    ret = read(fd, buffer, buffer_size);
    if (ret < 0) {
        if (errno != EAGAIN && errno != EWOULDBLOCK) {
            LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "读取导航串口数据错误: %s", strerror(errno));
            return -1;
        }
        return 0;  // 无数据可读
    }
    
    return ret;
}

/**
 * @brief 判断帧是否完整
 * 
 * @param frame 帧字符串
 * @param length 帧长度
 * @return 完整返回1，不完整返回0
 */
static int is_frame_complete(const char *frame, int length) {
    int i;
    
    // 检查帧长度
    if (length < 10) {
        return 0;
    }
    
    // 检查帧是否包含无效字符
    for (i = 0; i < length; i++) {
        if (frame[i] < 0x20 && frame[i] != '\r' && frame[i] != '\n') {
            // 包含控制字符（除了CR/LF），可能是二进制数据或噪声
            return 0;
        }
    }
    
    // 对于标准NMEA报文，结尾应为<CR><LF>，且必须包含校验和部分
    if (frame[0] == '$') {
        // 检查是否包含校验和部分"*XX"
        int has_checksum = 0;
        for (i = 0; i < length - 3; i++) {
            if (frame[i] == '*' && isxdigit(frame[i+1]) && isxdigit(frame[i+2])) {
                has_checksum = 1;
                break;
            }
        }
        
        // 检查是否以<CR><LF>结尾
        if (has_checksum && length >= 2 && frame[length - 2] == '\r' && frame[length - 1] == '\n') {
            return 1;
        }
    }
    
    // 对于GPFPD报文，格式类似NMEA，但需要特殊判断
    if (strncmp(frame, "$GPFPD", 6) == 0) {
        // 检查是否包含校验和部分"*XX"
        int has_checksum = 0;
        for (i = 0; i < length - 3; i++) {
            if (frame[i] == '*' && isxdigit(frame[i+1]) && isxdigit(frame[i+2])) {
                has_checksum = 1;
                break;
            }
        }
        
        // 检查是否以<CR><LF>结尾
        if (has_checksum && length >= 2 && frame[length - 2] == '\r' && frame[length - 1] == '\n') {
            return 1;
        }
    }
    
    // 对于INSPVAXA报文，需要有头部分号和校验和部分
    if (strncmp(frame, "#INSPVAXA", 9) == 0) {
        // 检查是否包含头部结束标志";"
        int has_header_end = 0;
        for (i = 9; i < length; i++) {
            if (frame[i] == ';') {
                has_header_end = 1;
                break;
            }
        }
        
        // 检查是否包含校验和部分"*XXXXXXXX"
        int has_checksum = 0;
        for (i = 0; i < length - 8; i++) {
            if (frame[i] == '*' && isxdigit(frame[i+1])) {
                has_checksum = 1;
                break;
            }
        }
        
        if (has_header_end && has_checksum) {
            return 1;
        }
    }
    
    // 对于IMU原始数据，通常以逗号分隔，第一个字段是数字，且有足够的逗号
    if (isdigit(frame[0])) {
        // 至少要有10个逗号（IMU数据至少有20个字段）
        int comma_count = 0;
        for (i = 0; i < length; i++) {
            if (frame[i] == ',') {
                comma_count++;
                if (comma_count >= 10) {
                    break;
                }
            }
        }
        
        if (comma_count >= 10) {
            // 检查是否包含结束标志（换行符或者回车换行）
            if ((length >= 1 && frame[length - 1] == '\n') || 
                (length >= 2 && frame[length - 2] == '\r' && frame[length - 1] == '\n')) {
                return 1;
            }
        }
    }
    
    return 0;
}

/**
 * @brief 解析导航数据帧
 * 
 * @param buffer 数据缓冲区
 * @param length 数据长度
 * @return 解析成功的帧数
 */
__attribute__((unused))
static int nav_parse_frame(uint8_t *buffer, int length) {
    int i;
    int frames_parsed = 0;
    char frame[NAV_FRAME_MAX_SIZE+1];  // +1用于确保有null结尾
    int frame_start = 0;
    int frame_len = 0;
    
    // 遍历接收到的数据，查找INSPVAXA帧头 '#'
    for (i = 0; i < length; i++) {
        if (buffer[i] == '#') {
            // 检查是否为INSPVAXA帧
            if (i + NAV_INSPVAXA_HEADER_LEN <= length && 
                strncmp((char *)&buffer[i], NAV_FRAME4_HEADER, NAV_INSPVAXA_HEADER_LEN) == 0) {
                
                // 找到了INSPVAXA帧头
                frame_start = i;
                frame_len = 0;
                
                // 收集帧数据直到行结束或达到最大帧长度
                while (i + frame_len < length && frame_len < NAV_FRAME_MAX_SIZE) {
                    frame[frame_len] = buffer[i + frame_len];
                    frame_len++;
                    
                    // 检查是否到达帧尾（CR+LF）
                    if (frame_len >= 2 && 
                        frame[frame_len-2] == '\r' && 
                        frame[frame_len-1] == '\n') {
                        break;
                    }
                }
                
                // 确保字符串以null结尾
                frame[frame_len] = '\0';
                
                // 检查帧是否完整
                if (is_frame_complete(frame, frame_len)) {
                    // 解析INSPVAXA帧
                    if (nav_parse_inspvaxa(frame) == 0) {
                        frames_parsed++;
                        last_data_time = time(NULL);  // 更新数据接收时间
                    } else {
                        LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "INSPVAXA帧解析失败");
                    }
                }
                
                // 调整索引，跳过已处理的数据
                i = frame_start + frame_len - 1;
            }
        }
    }
    
    return frames_parsed;
}

/**
 * @brief 解析RMC报文
 * 
 * @param frame 帧数据
 * @return int 成功返回1，失败返回0
 */
__attribute__((unused))
static int nav_parse_rmc(char *frame) {
    char *token;
    char *saveptr;
    int field_index = 0;
    nav_rmc_data_t temp_data = {0};
    
    // 校验帧格式
    if (!frame || strncmp(frame, "$GPRMC", 6) != 0) {
        return 0;
    }
    
    // 校验校验和
    if (!check_checksum(frame)) {
        LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "RMC帧校验和错误");
        return 0;
    }
    
    // 使用strtok_r解析字段，线程安全
    token = strtok_r(frame, ",*", &saveptr);
    while (token != NULL) {
        switch (field_index) {
            case 0:  // $GPRMC
                // 忽略帧头
                break;
            case 1:  // UTC时间
                if (strlen(token) >= 6) {
                    // 解析时间
                    temp_data.hour = (token[0] - '0') * 10 + (token[1] - '0');
                    temp_data.minute = (token[2] - '0') * 10 + (token[3] - '0');
                    temp_data.second = (token[4] - '0') * 10 + (token[5] - '0');
                    
                    // 如果有毫秒部分
                    if (strlen(token) > 7 && token[6] == '.') {
                        temp_data.millisecond = atof(token + 6) * 1000;
                    }
                }
                break;
            case 2:  // 定位状态
                temp_data.status = token[0];
                break;
            case 3:  // 纬度
                if (strlen(token) > 0) {
                    temp_data.latitude = parse_coordinate(token);
                }
                break;
            case 4:  // 南北半球
                temp_data.ns = token[0];
                break;
            case 5:  // 经度
                if (strlen(token) > 0) {
                    temp_data.longitude = parse_coordinate(token);
                }
                break;
            case 6:  // 东西半球
                temp_data.ew = token[0];
                break;
            case 7:  // 地面速率，节
                if (strlen(token) > 0) {
                    temp_data.speed = atof(token);
                }
                break;
            case 8:  // 地面航向，度
                if (strlen(token) > 0) {
                    temp_data.course = atof(token);
                }
                break;
            case 9:  // 日期，DDMMYY
                if (strlen(token) >= 6) {
                    temp_data.day = (token[0] - '0') * 10 + (token[1] - '0');
                    temp_data.month = (token[2] - '0') * 10 + (token[3] - '0');
                    temp_data.year = 2000 + (token[4] - '0') * 10 + (token[5] - '0');
                }
                break;
            case 12: // 模式指示
                temp_data.mode = token[0];
                break;
        }
        
        token = strtok_r(NULL, ",*", &saveptr);
        field_index++;
    }
    
    // 更新全局数据
    pthread_mutex_lock(&g_nav_data_mutex);
    
    temp_data.timestamp = (uint32_t)time(NULL);
    g_nav_rmc_data = temp_data;
    
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    LOG_DEBUG(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "解析RMC: 时间=%02d:%02d:%02d, 位置=%.6f%c %.6f%c",
              temp_data.hour, temp_data.minute, temp_data.second,
              temp_data.latitude, temp_data.ns, temp_data.longitude, temp_data.ew);
    
    return 1;
}

/**
 * @brief 解析GPGGA报文
 * 
 * @param frame 报文字符串
 * @return 成功返回0，失败返回-1
 */
__attribute__((unused))
static int nav_parse_gga(char *frame) {
    char *token;
    char *saveptr;
    int field_idx = 0;
    nav_gga_data_t temp_data = {0};
    char time_str[16] = {0};
    char lat_str[16] = {0};
    char lon_str[16] = {0};
    
    // 检查校验和
    if (!check_checksum(frame)) {
        return -1;
    }
    
    // 使用strtok_r解析报文
    token = strtok_r(frame, ",", &saveptr);
    
    while (token != NULL) {
        switch (field_idx) {
            case 0:  // 帧头 $GPGGA
                // 不需要处理
                break;
                
            case 1:  // UTC时间
                strncpy(time_str, token, sizeof(time_str) - 1);
                if (strlen(time_str) >= 6) {
                    temp_data.hour = (time_str[0] - '0') * 10 + (time_str[1] - '0');
                    temp_data.minute = (time_str[2] - '0') * 10 + (time_str[3] - '0');
                    temp_data.second = (time_str[4] - '0') * 10 + (time_str[5] - '0');
                    
                    // 解析毫秒部分（如果有）
                    if (strlen(time_str) > 7 && time_str[6] == '.') {
                        temp_data.millisecond = atof(time_str + 6) * 1000;
                    }
                }
                break;
                
            case 2:  // 纬度
                strncpy(lat_str, token, sizeof(lat_str) - 1);
                if (strlen(lat_str) > 0) {
                    temp_data.latitude = parse_coordinate(lat_str);
                }
                break;
                
            case 3:  // 北/南半球
                temp_data.ns = token[0];
                if (temp_data.ns == 'S') {
                    temp_data.latitude = -temp_data.latitude;
                }
                break;
                
            case 4:  // 经度
                strncpy(lon_str, token, sizeof(lon_str) - 1);
                if (strlen(lon_str) > 0) {
                    temp_data.longitude = parse_coordinate(lon_str);
                }
                break;
                
            case 5:  // 东/西半球
                temp_data.ew = token[0];
                if (temp_data.ew == 'W') {
                    temp_data.longitude = -temp_data.longitude;
                }
                break;
                
            case 6:  // 定位质量指示
                if (strlen(token) > 0) {
                    temp_data.quality = atoi(token);
                }
                break;
                
            case 7:  // 卫星数量
                if (strlen(token) > 0) {
                    temp_data.satellites = atoi(token);
                }
                break;
                
            case 8:  // 水平精度因子
                if (strlen(token) > 0) {
                    temp_data.hdop = atof(token);
                }
                break;
                
            case 9:  // 海拔高度
                if (strlen(token) > 0) {
                    temp_data.altitude = atof(token);
                }
                break;
                
            case 11:  // 大地水准面高度
                if (strlen(token) > 0) {
                    temp_data.geoid = atof(token);
                }
                break;
        }
        
        token = strtok_r(NULL, ",*", &saveptr);
        field_idx++;
    }
    
    // 添加时间戳
    temp_data.timestamp = (uint32_t)time(NULL);
    
    // 更新全局数据
    pthread_mutex_lock(&g_nav_data_mutex);
    g_nav_gga_data = temp_data;
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    return 0;
}

/**
 * @brief 解析GPVTG报文
 * 
 * @param frame 报文字符串
 * @return 成功返回0，失败返回-1
 */
__attribute__((unused))
static int nav_parse_vtg(char *frame) {
    char *token;
    char *saveptr;
    int field_idx = 0;
    nav_vtg_data_t temp_data = {0};
    
    // 检查校验和
    if (!check_checksum(frame)) {
        return -1;
    }
    
    // 使用strtok_r解析报文
    token = strtok_r(frame, ",", &saveptr);
    
    while (token != NULL) {
        switch (field_idx) {
            case 0:  // 帧头 $GPVTG
                // 不需要处理
                break;
                
            case 1:  // 真北航向
                if (strlen(token) > 0) {
                    temp_data.true_course = atof(token);
                }
                break;
                
            case 3:  // 磁北航向
                if (strlen(token) > 0) {
                    temp_data.magnetic_course = atof(token);
                }
                break;
                
            case 5:  // 地面速率（节）
                if (strlen(token) > 0) {
                    temp_data.speed_knots = atof(token);
                }
                break;
                
            case 7:  // 地面速率（公里/小时）
                if (strlen(token) > 0) {
                    temp_data.speed_kmh = atof(token);
                }
                break;
                
            case 9:  // 模式指示
                if (strlen(token) > 0) {
                    temp_data.mode = token[0];
                }
                break;
        }
        
        token = strtok_r(NULL, ",*", &saveptr);
        field_idx++;
    }
    
    // 添加时间戳
    temp_data.timestamp = (uint32_t)time(NULL);
    
    // 更新全局数据
    pthread_mutex_lock(&g_nav_data_mutex);
    g_nav_vtg_data = temp_data;
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    return 0;
}

/**
 * @brief 解析INSPVAXA报文
 * 
 * @param frame 报文字符串
 * @return 成功返回0，失败返回-1
 */
static int nav_parse_inspvaxa(char *frame) {
    char *token;
    char *saveptr;
    int field_idx = 0;
    nav_inspvaxa_data_t temp_data = {0};
    char *header_end;
    char *checksum_ptr;
    char header_buffer[NAV_FRAME_MAX_SIZE] = {0};
    char *header_token;
    char *header_saveptr;
    int header_field_idx = 0;
    
    // 找到头部结束标志";"
    header_end = strchr(frame, ';');
    if (header_end == NULL) {
        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "INSPVAXA格式错误: 找不到分隔符';'");
        return -1;
    }
    
    // 保存头部信息，方便调试
    int header_len = header_end - frame;
    if (header_len > 0 && header_len < NAV_FRAME_MAX_SIZE) {
        strncpy(header_buffer, frame, header_len);
        header_buffer[header_len] = '\0';
    }
    
    // 解析语句头信息
    header_token = strtok_r(header_buffer, ",", &header_saveptr);
    while (header_token != NULL && header_field_idx < 10) {
        switch (header_field_idx) {
            case 0:  // #INSPVAXA - 跳过
                break;
            case 1:  // 端口信息
                strncpy(temp_data.port, header_token, sizeof(temp_data.port) - 1);
                break;
            case 2:  // 保留字段
                break;
            case 3:  // 保留字段
                break;
            case 4:  // 解算类型
                strncpy(temp_data.solution_type, header_token, sizeof(temp_data.solution_type) - 1);
                break;
            case 5:  // GPS周数
                if (strlen(header_token) > 0) {
                    temp_data.week_number = atoi(header_token);
                }
                break;
            case 6:  // GPS周秒
                if (strlen(header_token) > 0) {
                    temp_data.week_seconds = atof(header_token);
                }
                break;
            case 7:  // 保留字段
                break;
            case 8:  // 保留字段
                break;
            case 9:  // 保留字段
                break;
        }
        header_token = strtok_r(NULL, ",", &header_saveptr);
        header_field_idx++;
    }
    
    // 检查校验和
    checksum_ptr = strrchr(frame, '*');
    if (checksum_ptr == NULL) {
        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "INSPVAXA格式错误: 找不到校验和'*'");
        return -1;
    }
    
    // 跳过头部，从状态字开始解析
    token = strtok_r(header_end + 1, ",", &saveptr);
    if (token == NULL) {
        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "INSPVAXA格式错误: 数据部分为空");
        return -1;
    }
    
    // 解析各字段
    while (token != NULL && field_idx < 24) {  // INSPVAXA有24个字段
        switch (field_idx) {
            case 0:  // INS状态字
                strncpy(temp_data.ins_status, token, sizeof(temp_data.ins_status) - 1);
                break;
                
            case 1:  // POS状态字
                strncpy(temp_data.pos_status, token, sizeof(temp_data.pos_status) - 1);
                break;
                
            case 2:  // 纬度
                if (strlen(token) > 0) {
                    temp_data.latitude = atof(token);
                    if (temp_data.latitude < -90.0 || temp_data.latitude > 90.0) {
                        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "INSPVAXA格式错误: 纬度超出范围 %f", temp_data.latitude);
                        temp_data.latitude = 0.0;  // 使用安全值
                    }
                }
                break;
                
            case 3:  // 经度
                if (strlen(token) > 0) {
                    temp_data.longitude = atof(token);
                    if (temp_data.longitude < -180.0 || temp_data.longitude > 180.0) {
                        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "INSPVAXA格式错误: 经度超出范围 %f", temp_data.longitude);
                        temp_data.longitude = 0.0;  // 使用安全值
                    }
                }
                break;
                
            case 4:  // 高程
                if (strlen(token) > 0) {
                    temp_data.height = atof(token);
                }
                break;
                
            case 5:  // 起伏
                if (strlen(token) > 0) {
                    temp_data.undulation = atof(token);
                }
                break;
                
            case 6:  // 北向速度
                if (strlen(token) > 0) {
                    temp_data.north_velocity = atof(token);
                }
                break;
                
            case 7:  // 东向速度
                if (strlen(token) > 0) {
                    temp_data.east_velocity = atof(token);
                }
                break;
                
            case 8:  // 天向速度
                if (strlen(token) > 0) {
                    temp_data.up_velocity = atof(token);
                }
                break;
                
            case 9:  // 横滚
                if (strlen(token) > 0) {
                    temp_data.roll = atof(token);
                }
                break;
                
            case 10:  // 俯仰
                if (strlen(token) > 0) {
                    temp_data.pitch = atof(token);
                }
                break;
                
            case 11:  // 航向
                if (strlen(token) > 0) {
                    temp_data.azimuth = atof(token);
                }
                break;
                
            case 12:  // 纬度标差
                if (strlen(token) > 0) {
                    temp_data.lat_std = atof(token);
                }
                break;
                
            case 13:  // 经度标差
                if (strlen(token) > 0) {
                    temp_data.lon_std = atof(token);
                }
                break;
                
            case 14:  // 高程标差
                if (strlen(token) > 0) {
                    temp_data.height_std = atof(token);
                }
                break;
                
            case 15:  // 北速标差
                if (strlen(token) > 0) {
                    temp_data.north_vel_std = atof(token);
                }
                break;
                
            case 16:  // 东速标差
                if (strlen(token) > 0) {
                    temp_data.east_vel_std = atof(token);
                }
                break;
                
            case 17:  // 天速标差
                if (strlen(token) > 0) {
                    temp_data.up_vel_std = atof(token);
                }
                break;
                
            case 18:  // 横滚标差
                if (strlen(token) > 0) {
                    temp_data.roll_std = atof(token);
                }
                break;
                
            case 19:  // 俯仰标差
                if (strlen(token) > 0) {
                    temp_data.pitch_std = atof(token);
                }
                break;
                
            case 20:  // 航向标差
                if (strlen(token) > 0) {
                    temp_data.azimuth_std = atof(token);
                }
                break;
                
            case 21:  // 扩展态
                if (strlen(token) > 0) {
                    temp_data.extended_status = atoi(token);
                }
                break;
                
            case 22:  // 更新时间
                if (strlen(token) > 0) {
                    temp_data.update_time = atoi(token);
                }
                break;
        }
        
        // 获取下一个字段，处理校验和
        if (field_idx == 22) {
            // 最后一个字段可能包含校验和
            char *checksum_in_token = strchr(token, '*');
            if (checksum_in_token != NULL) {
                *checksum_in_token = '\0';  // 截断校验和部分
            }
        }
        
        token = strtok_r(NULL, ",*", &saveptr);
        field_idx++;
    }
    
    // 字段数量检查
    if (field_idx < 23) {
        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "INSPVAXA格式错误: 字段不足，仅包含%d个字段", field_idx);
        // 如果字段太少但基本数据有效，仍可以尝试使用
        if (field_idx < 12) {
            return -1;
        }
    }
    
    // 添加时间戳
    temp_data.timestamp = (uint32_t)time(NULL);
    
    // 更新全局数据
    pthread_mutex_lock(&g_nav_data_mutex);
    g_nav_inspvaxa_data = temp_data;
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    // 每次获取到数据后直接打印（不进行去重判断）
    /* 
    LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, 
        "TU-BD100 INSPVAXA: INS=%s POS=%s 位置=[%.8f°,%.8f°,%.4fm] 速度=[N:%.4f E:%.4f U:%.4f]m/s 姿态=[R:%.6f° P:%.6f° Y:%.6f°] 精度=[位置:%.4f,%.4f,%.4f 速度:%.4f,%.4f,%.4f 姿态:%.6f,%.6f,%.6f] 扩展=%d 静止时间=%d",
        temp_data.ins_status, temp_data.pos_status,
        temp_data.latitude, temp_data.longitude, temp_data.height,
        temp_data.north_velocity, temp_data.east_velocity, temp_data.up_velocity,
        temp_data.roll, temp_data.pitch, temp_data.azimuth,
        temp_data.lat_std, temp_data.lon_std, temp_data.height_std,
        temp_data.north_vel_std, temp_data.east_vel_std, temp_data.up_vel_std,
        temp_data.roll_std, temp_data.pitch_std, temp_data.azimuth_std,
        temp_data.extended_status, temp_data.update_time);
    */    
    
    // 按照字段名称组织数据，便于查看
    LOG_INFO(LOG_TO_FILE, THREAD8_MODULE, 
        "=== TU-BD100 INSPVAXA 数据解析结果 ===\n"
        "语句头: 端口=%s, GPS周数=%d, GPS周秒=%.3f, 解算类型=%s\n"
        "INS状态字: %s\n"
        "POS状态字: %s\n"
        "纬度: %.8f°\n"
        "经度: %.8f°\n"
        "高程: %.4fm\n"
        "起伏(椭球分离值): %.4fm\n"
        "北向速度: %.4fm/s\n"
        "东向速度: %.4fm/s\n"
        "天向速度: %.4fm/s\n"
        "横滚: %.6f°\n"
        "俯仰: %.6f°\n"
        "航向: %.6f°\n"
        "纬度标差: %.4fm\n"
        "经度标差: %.4fm\n"
        "高程标差: %.4fm\n"
        "北速标差: %.4fm/s\n"
        "东速标差: %.4fm/s\n"
        "天速标差: %.4fm/s\n"
        "横滚标差: %.6f°\n"
        "俯仰标差: %.6f°\n"
        "航向标差: %.6f°\n"
        "扩展态: %d\n"
        "更新时间: %d\n"
        "=== 数据解析完成 ===",
        temp_data.port, temp_data.week_number, temp_data.week_seconds, temp_data.solution_type,
        temp_data.ins_status, temp_data.pos_status,
        temp_data.latitude, temp_data.longitude, temp_data.height, temp_data.undulation,
        temp_data.north_velocity, temp_data.east_velocity, temp_data.up_velocity,
        temp_data.roll, temp_data.pitch, temp_data.azimuth,
        temp_data.lat_std, temp_data.lon_std, temp_data.height_std,
        temp_data.north_vel_std, temp_data.east_vel_std, temp_data.up_vel_std,
        temp_data.roll_std, temp_data.pitch_std, temp_data.azimuth_std,
        temp_data.extended_status, temp_data.update_time);
    
    return 0;
}

/**
 * @brief 解析GPFPD报文
 * 
 * @param frame 报文字符串
 * @return 成功返回0，失败返回-1
 */
__attribute__((unused))
static int nav_parse_gpfpd(char *frame) {
    char *token;
    char *saveptr;
    int field_idx = 0;
    nav_gpfpd_data_t temp_data = {0};
    
    // 检查校验和
    if (!check_checksum(frame)) {
        return -1;
    }
    
    // 使用strtok_r解析报文
    token = strtok_r(frame, ",", &saveptr);
    
    while (token != NULL) {
        switch (field_idx) {
            case 0:  // 帧头 $GPFPD
                // 不需要处理
                break;
                
            case 1:  // GPS周
                if (strlen(token) > 0) {
                    temp_data.gps_week = atoi(token);
                }
                break;
                
            case 2:  // GPS周秒
                if (strlen(token) > 0) {
                    temp_data.gps_seconds = atof(token);
                }
                break;
                
            case 3:  // 偏航角
                if (strlen(token) > 0) {
                    temp_data.heading = atof(token);
                }
                break;
                
            case 4:  // 俯仰角
                if (strlen(token) > 0) {
                    temp_data.pitch = atof(token);
                }
                break;
                
            case 5:  // 滚动角
                if (strlen(token) > 0) {
                    temp_data.roll = atof(token);
                }
                break;
                
            case 6:  // 纬度
                if (strlen(token) > 0) {
                    temp_data.latitude = atof(token);
                }
                break;
                
            case 7:  // 经度
                if (strlen(token) > 0) {
                    temp_data.longitude = atof(token);
                }
                break;
                
            case 8:  // 高度
                if (strlen(token) > 0) {
                    temp_data.altitude = atof(token);
                }
                break;
                
            case 9:  // 东速
                if (strlen(token) > 0) {
                    temp_data.east_vel = atof(token);
                }
                break;
                
            case 10:  // 北速
                if (strlen(token) > 0) {
                    temp_data.north_vel = atof(token);
                }
                break;
                
            case 11:  // 天速
                if (strlen(token) > 0) {
                    temp_data.up_vel = atof(token);
                }
                break;
                
            case 12:  // 基线长度
                if (strlen(token) > 0) {
                    temp_data.baseline = atof(token);
                }
                break;
                
            case 13:  // 天线1卫星数
                if (strlen(token) > 0) {
                    temp_data.nsv1 = atoi(token);
                }
                break;
                
            case 14:  // 天线2卫星数
                if (strlen(token) > 0) {
                    temp_data.nsv2 = atoi(token);
                }
                break;
                
            case 15:  // 卫星状态
                if (strlen(token) > 0) {
                    temp_data.satellite_status = atoi(token);
                }
                break;
        }
        
        token = strtok_r(NULL, ",*", &saveptr);
        field_idx++;
    }
    
    // 添加时间戳
    temp_data.timestamp = (uint32_t)time(NULL);
    
    // 更新全局数据
    pthread_mutex_lock(&g_nav_data_mutex);
    g_nav_gpfpd_data = temp_data;
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    return 0;
}

/**
 * @brief 解析IMU原始数据
 * 
 * @param frame 报文字符串
 * @return 成功返回0，失败返回-1
 */
__attribute__((unused))
static int nav_parse_imu_data(char *frame) {
    char *token;
    char *saveptr;
    int field_idx = 0;
    nav_imu_data_t temp_data = {0};
    
    // IMU原始数据是以逗号分隔的ASCII格式，无需校验和检查
    
    // 使用strtok_r解析报文
    token = strtok_r(frame, ",", &saveptr);
    
    while (token != NULL && field_idx < 31) {  // IMU数据有31个字段
        switch (field_idx) {
            case 0:  // 帧计数
                if (strlen(token) > 0) {
                    temp_data.frame_count = atoi(token);
                }
                break;
                
            case 1:  // X轴陀螺(原始)
                if (strlen(token) > 0) {
                    temp_data.gyro_x_raw = atof(token);
                }
                break;
                
            case 2:  // Y轴陀螺(原始)
                if (strlen(token) > 0) {
                    temp_data.gyro_y_raw = atof(token);
                }
                break;
                
            case 3:  // Z轴陀螺(原始)
                if (strlen(token) > 0) {
                    temp_data.gyro_z_raw = atof(token);
                }
                break;
                
            case 4:  // X轴加速度计(原始)
                if (strlen(token) > 0) {
                    temp_data.accel_x_raw = atof(token);
                }
                break;
                
            case 5:  // Y轴加速度计(原始)
                if (strlen(token) > 0) {
                    temp_data.accel_y_raw = atof(token);
                }
                break;
                
            case 6:  // Z轴加速度计(原始)
                if (strlen(token) > 0) {
                    temp_data.accel_z_raw = atof(token);
                }
                break;
                
            case 7:  // IMU温度(度)
                if (strlen(token) > 0) {
                    temp_data.temperature = atof(token);
                }
                break;
                
            case 8:  // IMU计数
                if (strlen(token) > 0) {
                    temp_data.imu_count = atoi(token);
                }
                break;
                
            case 9:  // IMU标志
                if (strlen(token) > 0) {
                    temp_data.imu_flag = atoi(token);
                }
                break;
                
            case 10:  // X轴陀螺(标定后,度/S)
                if (strlen(token) > 0) {
                    temp_data.gyro_x_cal = atof(token);
                }
                break;
                
            case 11:  // Y轴陀螺(标定后,度/S)
                if (strlen(token) > 0) {
                    temp_data.gyro_y_cal = atof(token);
                }
                break;
                
            case 12:  // Z轴陀螺(标定后,度/S)
                if (strlen(token) > 0) {
                    temp_data.gyro_z_cal = atof(token);
                }
                break;
                
            case 13:  // X轴加速度计(标定后,g)
                if (strlen(token) > 0) {
                    temp_data.accel_x_cal = atof(token);
                }
                break;
                
            case 14:  // Y轴加速度计(标定后,g)
                if (strlen(token) > 0) {
                    temp_data.accel_y_cal = atof(token);
                }
                break;
                
            case 15:  // Z轴加速度计(标定后,g)
                if (strlen(token) > 0) {
                    temp_data.accel_z_cal = atof(token);
                }
                break;
                
            case 16:  // PPS计数
                if (strlen(token) > 0) {
                    temp_data.pps_count = atoi(token);
                }
                break;
                
            case 17:  // PPS时间(ms)
                if (strlen(token) > 0) {
                    temp_data.pps_time = atoi(token);
                }
                break;
                
            case 18:  // PPS更新标志
                if (strlen(token) > 0) {
                    temp_data.pps_update_flag = atoi(token);
                }
                break;
                
            case 19:  // 气压计(hPa)
                if (strlen(token) > 0) {
                    temp_data.barometer = atof(token);
                }
                break;
                
            case 20:  // 气压计更新标志
                if (strlen(token) > 0) {
                    temp_data.baro_update_flag = atoi(token);
                }
                break;
        }
        
        token = strtok_r(NULL, ",\r\n", &saveptr);
        field_idx++;
    }
    
    // 帧计数校验和统计
    g_total_imu_frames++;
    
    // 检查是否需要初始化统计时间
    time_t current_time = time(NULL);
    if (g_imu_stat_time == 0) {
        g_imu_stat_time = current_time;
    }
    
    // 检查帧计数是否连续
    if (g_last_frame_count > 0 && temp_data.frame_count > 0) {
        // 如果帧计数回环(大于65000跳转到0附近)
        if (g_last_frame_count > 65000 && temp_data.frame_count < 1000) {
            // 正常回环，不视为丢帧
            LOG_DEBUG(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "IMU数据帧计数回环: %d -> %d", g_last_frame_count, temp_data.frame_count);
        } 
        // 正常递增1的情况
        else if (temp_data.frame_count == g_last_frame_count + 1) {
            // 正常帧，不做处理
        } 
        // 异常情况
        else {
            int lost_frames = 0;
            
            // 计算丢失的帧数
            if (temp_data.frame_count > g_last_frame_count) {
                lost_frames = temp_data.frame_count - g_last_frame_count - 1;
            } else {
                // 考虑回环情况，假设最大帧计数为65535
                lost_frames = (65536 + temp_data.frame_count - g_last_frame_count - 1) % 65536;
            }
            
            // 累计丢帧数
            g_imu_lost_frames += lost_frames;
            
            // 记录丢帧信息
            LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "IMU数据丢帧: 上一帧=%d, 当前帧=%d, 丢失=%d帧", 
                   g_last_frame_count, temp_data.frame_count, lost_frames);
        }
    }
    
    // 更新上一帧计数
    g_last_frame_count = temp_data.frame_count;
    
    // 每隔60秒输出一次统计信息
    if (current_time - g_imu_stat_time >= 60) {
        double loss_rate = 0.0;
        if (g_total_imu_frames > 0) {
            loss_rate = (double)g_imu_lost_frames / (g_total_imu_frames + g_imu_lost_frames) * 100.0;
        }
        
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "IMU数据统计(60s): 接收=%d帧, 丢失=%d帧, 丢包率=%.2f%%", 
               g_total_imu_frames, g_imu_lost_frames, loss_rate);
        
        // 如果丢包率过高，可能需要进行错误处理
        if (loss_rate > 10.0) {
            LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "警告: IMU数据丢包率过高(%.2f%%)，通信可能不稳定", loss_rate);
        }
        
        // 重置统计
        g_imu_stat_time = current_time;
        g_total_imu_frames = 0;
        g_imu_lost_frames = 0;
    }
    
    // 添加时间戳
    temp_data.timestamp = (uint32_t)time(NULL);
    
    // 更新全局数据
    pthread_mutex_lock(&g_nav_data_mutex);
    g_nav_imu_data = temp_data;
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    return 0;
}

/**
 * @brief 解析坐标字符串（NMEA格式）为度数
 * 
 * @param str 坐标字符串（格式：ddmm.mmmm）
 * @return 度数
 */
static double parse_coordinate(const char *str) {
    double value = atof(str);
    int degrees = (int)(value / 100);
    double minutes = value - degrees * 100;
    return degrees + minutes / 60.0;
}

/**
 * @brief 检查NMEA报文校验和
 * 
 * @param frame 完整的NMEA报文字符串
 * @return 校验和正确返回1，错误返回0
 */
static int check_checksum(const char *frame) {
    unsigned char calculated_checksum = 0;
    int i = 0;
    char checksum_str[3] = {0};
    unsigned int received_checksum = 0;
    char *checksum_ptr;
    
    // 查找校验和部分
    checksum_ptr = strchr(frame, '*');
    if (checksum_ptr == NULL || strlen(checksum_ptr) < 3) {
        return 0;  // 无校验和或格式错误
    }
    
    // 提取校验和字符串
    strncpy(checksum_str, checksum_ptr + 1, 2);
    checksum_str[2] = '\0';
    
    // 将校验和字符串转换为数值
    sscanf(checksum_str, "%x", &received_checksum);
    
    // 计算校验和（从$后的第一个字符开始，到*之前的所有字符的异或值）
    for (i = 1; frame[i] != '*' && frame[i] != '\0'; i++) {
        calculated_checksum ^= frame[i];
    }
    
    // 比较校验和
    return (calculated_checksum == received_checksum);
}

/**
 * @brief 获取RMC数据
 * 
 * @param data 数据结构体指针
 * @return 成功返回0，失败返回-1
 */
int get_nav_rmc_data(nav_rmc_data_t *data) {
    if (data == NULL) {
        return -1;
    }
    
    pthread_mutex_lock(&g_nav_data_mutex);
    *data = g_nav_rmc_data;
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    return 0;
}

/**
 * @brief 获取GGA数据
 * 
 * @param data 数据结构体指针
 * @return 成功返回0，失败返回-1
 */
int get_nav_gga_data(nav_gga_data_t *data) {
    if (data == NULL) {
        return -1;
    }
    
    pthread_mutex_lock(&g_nav_data_mutex);
    *data = g_nav_gga_data;
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    return 0;
}

/**
 * @brief 获取VTG数据
 * 
 * @param data 数据结构体指针
 * @return 成功返回0，失败返回-1
 */
int get_nav_vtg_data(nav_vtg_data_t *data) {
    if (data == NULL) {
        return -1;
    }
    
    pthread_mutex_lock(&g_nav_data_mutex);
    *data = g_nav_vtg_data;
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    return 0;
}

/**
 * @brief 获取INSPVAXA数据
 * 
 * @param data 数据结构体指针
 * @return 成功返回0，失败返回-1
 */
int get_nav_inspvaxa_data(nav_inspvaxa_data_t *data) {
    if (data == NULL) {
        return -1;
    }
    
    pthread_mutex_lock(&g_nav_data_mutex);
    *data = g_nav_inspvaxa_data;
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    return 0;
}

/**
 * @brief 获取GPFPD数据
 * 
 * @param data 数据结构体指针
 * @return 成功返回0，失败返回-1
 */
int get_nav_gpfpd_data(nav_gpfpd_data_t *data) {
    if (data == NULL) {
        return -1;
    }
    
    pthread_mutex_lock(&g_nav_data_mutex);
    *data = g_nav_gpfpd_data;
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    return 0;
}

/**
 * @brief 获取IMU原始数据
 * 
 * @param data 数据结构体指针
 * @return 成功返回0，失败返回-1
 */
int get_nav_imu_data(nav_imu_data_t *data) {
    if (data == NULL) {
        return -1;
    }
    
    pthread_mutex_lock(&g_nav_data_mutex);
    *data = g_nav_imu_data;
    pthread_mutex_unlock(&g_nav_data_mutex);
    
    return 0;
}

/**
 * @brief 检测帧数据是否接收完毕
 * 
 * @param fd 串口文件描述符
 * @param buffer 数据缓冲区
 * @param buffer_size 缓冲区大小
 * @param received_length 已接收的数据长度指针
 * @return 成功返回0，失败返回-1
 */
static int wait_for_frame_complete(int fd, uint8_t *buffer, int buffer_size, int *received_length) {
    int total_length = 0;
    int check_count = 0;
    int last_length = 0;
    
    if (fd < 0 || buffer == NULL || received_length == NULL) {
        return -1;
    }
    
    *received_length = 0;
    
    while (check_count < FRAME_COMPLETE_CHECK_COUNT && total_length < buffer_size - 1) {
        // 尝试读取数据
        int bytes_read = read(fd, buffer + total_length, buffer_size - total_length - 1);
        
        if (bytes_read > 0) {
            total_length += bytes_read;
            last_length = total_length;
            check_count = 0;  // 重置检测计数，因为有新数据
        } else if (bytes_read == 0 || (bytes_read < 0 && (errno == EAGAIN || errno == EWOULDBLOCK))) {
            // 无数据可读，检查是否数据长度没有变化
            if (total_length == last_length) {
                check_count++;
            } else {
                check_count = 0;
                last_length = total_length;
            }
        } else {
            // 读取错误
            LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "读取串口数据错误: %s", strerror(errno));
            return -1;
        }
        
        // 等待间隔
        usleep(FRAME_COMPLETE_CHECK_INTERVAL_MS * 1000);
    }
    
    // 确保字符串以null结尾
    buffer[total_length] = '\0';
    *received_length = total_length;
    
    return 0;
}

/**
 * @brief 从接收到的数据中提取INSPVAXA帧
 * 
 * @param buffer 数据缓冲区
 * @param length 数据长度
 * @param frame_buffer 输出帧缓冲区
 * @param frame_buffer_size 帧缓冲区大小
 * @return 成功返回帧长度，失败返回-1
 */
static int extract_inspvaxa_frame(const uint8_t *buffer, int length, char *frame_buffer, int frame_buffer_size) {
    const char *frame_header = "#INSPVAXA";
    const int header_len = 9;
    int i, j;
    
    if (buffer == NULL || frame_buffer == NULL || length <= 0) {
        return -1;
    }
    
    // 查找INSPVAXA帧头
    for (i = 0; i <= length - header_len; i++) {
        if (memcmp(buffer + i, frame_header, header_len) == 0) {
            // 找到帧头，查找帧尾（\r\n）
            for (j = i + header_len; j < length - 1; j++) {
                if (buffer[j] == '\r' && buffer[j + 1] == '\n') {
                    // 找到完整帧
                    int frame_length = j - i + 2;
                    if (frame_length < frame_buffer_size) {
                        memcpy(frame_buffer, buffer + i, frame_length);
                        frame_buffer[frame_length] = '\0';
                        return frame_length;
                    }
                    break;
                }
            }
        }
    }
    
    return -1;
}

/**
 * @brief 关闭导航串口
 * 
 * @param fd 串口文件描述符
 */
void nav_rs232_close(int fd) {
    if (fd >= 0) {
        close(fd);
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "TU-BD100串口已关闭");
    }
}

/**
 * @brief 停止线程8运行
 */
void thread8_stop(void) {
    g_thread_running = 0;
}

/**
 * @brief 线程8主函数：TU-BD100组合导航数据接收与处理
 * 
 * @param arg 线程参数（未使用）
 * @return void* 线程返回值（未使用）
 */
void *thread8_function(void *arg) {
    (void)arg;
    uint8_t *rx_buffer = NULL;
    char frame_buffer[NAV_FRAME_MAX_SIZE + 1];
    int received_length;
    int retry_count = 0;
    struct timeval timeout;
    fd_set readfds;
    int select_result;
    
    LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "TU-BD100组合导航数据接收线程启动");
    
    // 分配接收缓冲区
    rx_buffer = (uint8_t *)malloc(NAV_RX_BUFFER_SIZE);
    if (rx_buffer == NULL) {
        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "内存分配失败，线程退出");
        return NULL;
    }
    
    // 初始化全局变量
    memset(&g_nav_inspvaxa_data, 0, sizeof(g_nav_inspvaxa_data));
    
    // 初始化串口
    g_nav_rs232_fd = nav_rs232_init(NAV_RS232_DEVICE, NAV_RS232_BAUDRATE);
    if (g_nav_rs232_fd < 0) {
        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "初始化TU-BD100串口失败，线程退出");
        free(rx_buffer);
        return NULL;
    }
    
    // 等待串口稳定
    usleep(TU_BD100_INIT_WAIT_SEC * 1000000);
    
    // 发送INSPVAXA配置命令：log com1 inspvaxa ontime 2
    const char *config_cmd = TU_BD100_CONFIG_CMD;
    int cmd_len = strlen(config_cmd);
    int bytes_written = write(g_nav_rs232_fd, config_cmd, cmd_len);
    
    if (bytes_written == cmd_len) {
        LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "已发送TU-BD100配置命令: %s", TU_BD100_CONFIG_CMD);
    } else {
        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "发送TU-BD100配置命令失败: %s", strerror(errno));
    }
    
    // 等待配置生效
    usleep(TU_BD100_CONFIG_WAIT_SEC * 1000000);
    
    // 清空串口缓冲区
    tcflush(g_nav_rs232_fd, TCIOFLUSH);
    LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "已清空串口缓冲区");
    
    LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "开始接收TU-BD100 INSPVAXA数据...");
    
    // 主循环
    while (g_thread_running) {
        // 使用select检测是否有数据可读
        FD_ZERO(&readfds);
        FD_SET(g_nav_rs232_fd, &readfds);
        timeout.tv_sec = TU_BD100_DATA_INTERVAL_SEC + 1;  // 超时时间设为数据间隔+1秒
        timeout.tv_usec = 0;
        
        select_result = select(g_nav_rs232_fd + 1, &readfds, NULL, NULL, &timeout);
        
        if (select_result == -1) {
            // 选择错误
            if (errno != EINTR) {
                LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "select TU-BD100串口错误: %s", strerror(errno));
                retry_count++;
                if (retry_count > 10) {
                    LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "TU-BD100串口读取多次失败，正在重新初始化...");
                    nav_rs232_close(g_nav_rs232_fd);
                    g_nav_rs232_fd = nav_rs232_init(NAV_RS232_DEVICE, NAV_RS232_BAUDRATE);
                    if (g_nav_rs232_fd < 0) {
                        LOG_ERROR(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "重新初始化TU-BD100串口失败，线程退出");
                        break;
                    }
                    
                    // 重新发送配置命令
                    if (write(g_nav_rs232_fd, TU_BD100_CONFIG_CMD, strlen(TU_BD100_CONFIG_CMD)) < 0) {
                        LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "重新发送配置命令失败: %s", strerror(errno));
                    }
                    usleep(TU_BD100_CONFIG_WAIT_SEC * 1000000);
                    tcflush(g_nav_rs232_fd, TCIOFLUSH);
                    
                    retry_count = 0;
                }
            }
            continue;
        } else if (select_result == 0) {
            // 超时，无数据可读
            static time_t last_warning_time = 0;
            time_t now = time(NULL);
            
            // 检查是否长时间无数据
            if (last_data_time > 0 && now - last_data_time > TU_BD100_NO_DATA_TIMEOUT_SEC) {
                if (now - last_warning_time >= TU_BD100_WARNING_INTERVAL_SEC) {
                    LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "长时间无TU-BD100数据接收");
                    last_warning_time = now;
                }
            }
            
            continue;
        }
        
        // 有数据可读，使用新的接收逻辑
        if (wait_for_frame_complete(g_nav_rs232_fd, rx_buffer, NAV_RX_BUFFER_SIZE, &received_length) == 0 && received_length > 0) {
            // 成功接收数据，重置错误计数
            retry_count = 0;
            last_data_time = time(NULL);
            
            // 记录原始数据到日志文件（INFO级别）
            LOG_INFO(LOG_TO_FILE, THREAD8_MODULE, "导航仪原始数据(%d字节): %s", received_length, (char*)rx_buffer);
            
            // 打印接收到的原始数据（用于调试）
            LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "接收到原始数据(%d字节): %s", received_length, (char*)rx_buffer);
            
            // 提取INSPVAXA帧
            int frame_length = extract_inspvaxa_frame(rx_buffer, received_length, frame_buffer, sizeof(frame_buffer));
            if (frame_length > 0) {
                LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "提取到INSPVAXA帧(%d字节): %s", frame_length, frame_buffer);
                
                // 解析INSPVAXA帧
                if (nav_parse_inspvaxa(frame_buffer) == 0) {
                    LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "INSPVAXA帧解析成功");
                } else {
                    LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "INSPVAXA帧解析失败");
                }
            } else {
                LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "未找到有效的INSPVAXA帧");
            }
        } else {
            LOG_WARN(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "数据接收失败或无数据");
        }
    }
    
    // 清理资源
    nav_rs232_close(g_nav_rs232_fd);
    g_nav_rs232_fd = -1;
    
    if (rx_buffer) {
        free(rx_buffer);
    }
    
    LOG_INFO(LOG_TO_CONSOLE_ONLY, THREAD8_MODULE, "TU-BD100组合导航数据接收线程退出");
    return NULL;
} 